/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2023-02-21     Wangjiangbo       the first version
 */
#include <Force/Force.h>
#include "bsp/bsp_can1.h"

#define DBG_TAG "force"
#define DBG_LVL DBG_LOG
#include <rtdbg.h>


void force_init(void)
{

}

bool force_get(int32_t* array)
{
    struct rt_can_msg msg;
    int8_t retry = 1;
    int16_t flag = 0;
    int32_t axit[6] = {0};

    while(retry != 0)
    {
        msg.ide = RT_CAN_EXTID;        // 标准格式
        msg.rtr = RT_CAN_DTR;          // 数据帧
        msg.len = 5;                   // 数据长度
        msg.id = 0x20110;

        memset(msg.data, 0, sizeof(msg.data));

        msg.data[0] = 3;
        msg.data[1] = 2;
        msg.data[2] = 0;
        msg.data[3] = 0x12;
        msg.data[4] = 1;

        bsp_can1_tx_transmit(&msg);

        flag = 0;

        while(1)
        {
            if(bsp_can1_rx_transmit(&msg, 30) == false)
                break;

            if(msg.id == 0x10220 && msg.len == 8)
            {
                flag |= 0x01;
                axit[0] = ((msg.data[2] << 16) | (msg.data[3] << 8) | (msg.data[4]));
                axit[1] = ((msg.data[5] << 16) | (msg.data[6] << 8) | (msg.data[7]));
            }
            else if(msg.id == 0x10241 && msg.len == 8)
            {
                flag |= 0x02;
                axit[2] = ((msg.data[0] << 16) | (msg.data[1] << 8) | (msg.data[2]));
                axit[3] = ((msg.data[3] << 16) | (msg.data[4] << 8) | (msg.data[5]));
                axit[4] = ((msg.data[6] << 16) | (msg.data[7] << 8));
            }
            else if(msg.id == 0x10282 && msg.len == 4)
            {
                flag |= 0x04;
                axit[4] |= (msg.data[0]);
                axit[5] = ((msg.data[1] << 16) | (msg.data[2] << 8) | (msg.data[3]));
            }

            if(flag == 7)
            {
//                LOG_D("raw fx = %08d fy = %08d fz = %08d tx = %08d",  axit[0],  axit[1],  axit[2],  axit[3] );

                for(int i = 0; i < 6 ; i++)
                {
                    if(axit[i] & 0x800000)
                    {
                       axit[i] &= 0x7ffff;
                       axit[i] = 0 - axit[i];
                    }

                    array[i] = axit[i];
                }
//                LOG_D("fx = %08d fy = %08d fz = %08d tx = %08d",  axit[0],  axit[1],  axit[2],  axit[3] );
                return true;
            }
        }
        retry--;
    }
    return false;
}

/*

    发送：
    0x00020110 0x03 0x02 0x00 0x12 0x01
    应答：
        邮箱标识符       功能码     寄存器长度 传感器采集数据
    0x00010220    0x10     0x12      Fx    Fy
    邮箱标识符           传感器采集数据
    0x00010241     Fz Tx Ty(最高位和中间位字节)
    邮箱标识符           传感器采集数据
    0x00010282     Ty(最低位字节) Tz

 */
